/*
 * ps2_communication.c
 *
 *  Created on: 2025年11月1日
 *      Author: 29795
 */
#include "ps2_communication.h"

//执行片选
void ps2_cs(void)
{
    //设置CS口为低电平
    R_BSP_PinWrite(PS2_CS,BSP_IO_LEVEL_LOW);
}

//取消片选
void ps2_ncs(void)
{
    //设置CS口为高电平
    R_BSP_PinWrite(PS2_CS,BSP_IO_LEVEL_HIGH);
}

//时钟下降沿执行
void ps2_clk_down(void)
{
    //设置CLK为低电平
    R_BSP_PinWrite(PS2_CLK,BSP_IO_LEVEL_LOW);
}

//时钟上升沿执行
void ps2_clk_up(void)
{
    //设置CLK为高电平
    R_BSP_PinWrite(PS2_CLK,BSP_IO_LEVEL_HIGH);
}

//读取DI口
uint8_t ps2_read(void)
{
    //读取DI口
    return (uint8_t)R_BSP_PinRead(PS2_DI);
}

//写入DO口
void ps2_write(uint8_t data)
{
    //写入DO口
    R_BSP_PinWrite(PS2_DO,(data>0));
}

//单个字节读写数据
uint8_t ps2_byte(uint8_t data)
{
    uint8_t byte_get = 0;
    for(uint8_t i = 0; i < 8; i ++)
    {
        ps2_clk_up();
        ps2_write(data&0x1);
        R_BSP_SoftwareDelay(10, BSP_DELAY_UNITS_MICROSECONDS);

        ps2_clk_down();
        byte_get = byte_get|(uint8_t)(ps2_read()<<i);
        R_BSP_SoftwareDelay(10, BSP_DELAY_UNITS_MICROSECONDS);
        data >>= 1;
    }
    return byte_get;
}

//完整PS2通讯流程
void ps2_communication(uint8_t data_array[9])
{
    ps2_cs();
    //通过串口发送信息
    data_array[0] = ps2_byte(0x01);
    data_array[1] = ps2_byte(0x42);
    data_array[2] = ps2_byte(0xFF);
    data_array[3] = ps2_byte(0xFF);
    data_array[4] = ps2_byte(0xFF);
    data_array[5] = ps2_byte(0xFF);
    data_array[6] = ps2_byte(0xFF);
    data_array[7] = ps2_byte(0xFF);
    data_array[8] = ps2_byte(0xFF);
    ps2_ncs();
}

//检测是否存在变档
uint8_t ps2_detection(uint8_t data_array[9])
{
    //第5字节的低四位如果不为0x05则表示存在变档
    return ((data_array[4]&0x05) != 0x05);
}

//flag变换函数
uint8_t ps2_flag_change(uint8_t* byte,uint8_t *ps2_flag, uint8_t mask, uint8_t aim)
{
    if(((*byte)&mask) == 0)
    {
        if(ps2_flag != NULL)
        {
            *ps2_flag = aim;
        }
        return 1;
    }
    return 0;
}

//档位变换
void ps2_gear_change(float *aim_data, float gear_step, float max_value)
{
    *aim_data += gear_step;
    if(*aim_data > max_value)
    {
        *aim_data = gear_step;
    }
}

#define ps2_theta_step 0.6f
#define ps2_v_step 0.1f
#define ps2_theta_max 3.14f
#define ps2_v_max 0.6f
uint8_t ps2_byte_data = 0;
float ps2_v_abs = ps2_v_step;
float ps2_theta_abs = ps2_theta_step;
uint8_t v_flag = 0;
uint8_t theta_flag = 0;
//1表示遥控，0表示自主避障
uint8_t ps2_remote_control_mode = 1;

uint8_t ps2_get_remote_control_mode(void)
{
    return ps2_remote_control_mode;
}

//基于字节产生速度决策，基于高低4位
void ps2_byte_decision(uint8_t byte, float decision[2], uint8_t is_high)
{

    //判断是否为高低
    if(is_high == 1)
    {
        //处理高4位
        byte = byte >> 4;
        byte = byte | 0xF0;
        ps2_byte_data = ~byte;
    }
    else
    {
        //处理低4位
        byte = byte | 0xF0;
        ps2_byte_data = ~byte;
    }

    //根据数据生成决策
    if(ps2_byte_data&0x01)
    {
        decision[0] = ps2_v_abs;
    }
    else if(ps2_byte_data&0x02)
    {
        decision[1] = ps2_theta_abs;
    }
    else if(ps2_byte_data&0x04)
    {
        decision[0] = ps2_v_abs * -1.0f;
    }
    else if(ps2_byte_data&0x08)
    {
        decision[1] = ps2_theta_abs * -1.0f;
    }
}

//基于通讯内容生成决策
void ps2_decision_generation(float decision[2])
{
    //初始化
    decision[0] = 0.0f;
    decision[1] = 0.0f;

    //获取数据
    static uint8_t ps2_array[9] = {0};
    ps2_communication(ps2_array);
    if(ps2_array[1] != 0x41)
    {
        //信号丢失则重置为0;
        decision[0] = 0.0f;
        decision[1] = 0.0f;
        ps2_remote_control_mode = 1;
        return;
    }
    while(ps2_detection(ps2_array))
    {
        ps2_flag_change(ps2_array+4, &v_flag, 0x01, 1);
        ps2_flag_change(ps2_array+4, &theta_flag, 0x04, 1);
//        ps2_flag_change(ps2_array+4, &ps2_remote_control_mode, 0x08, 1);
//        ps2_flag_change(ps2_array+4, &ps2_remote_control_mode, 0x02, 0);
        ps2_communication(ps2_array);
    }

    ps2_remote_control_mode = 1;
    if(ps2_flag_change(ps2_array+4, NULL, 0x08, 0) ||
            ps2_flag_change(ps2_array+4, NULL, 0x02, 0))
    {
        ps2_remote_control_mode = 0;
    }

    if(ps2_remote_control_mode == 0)
    {
        return;
    }

    //是否变换档位
    if(v_flag == 1)
    {
        ps2_gear_change(&ps2_v_abs,ps2_v_step,ps2_v_max);
        v_flag = 0;
    }
    if(theta_flag == 1)
    {
        ps2_gear_change(&ps2_theta_abs,ps2_theta_step,ps2_theta_max);
        theta_flag = 0;
    }
    
    ps2_byte_decision(ps2_array[3],decision,1);
    ps2_byte_decision(ps2_array[4],decision,1);

}
